/*
 * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "soc/soc_caps.h"
#include "esp_log.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"
#include "esp_adc/adc_continuous.h"
#include <math.h>

#include "driver_adc.h"

const static char *TAG = "driver_adc";

/*---------------------------------------------------------------
        ADC General Macros
---------------------------------------------------------------*/
#if 0//ADC1 Channels

#define EXAMPLE_ADC_ATTEN           ADC_ATTEN_DB_12

const static uint8_t  adc_channel[] = {EXAMPLE_ADC1_CHAN3, EXAMPLE_ADC1_CHAN4, EXAMPLE_ADC1_CHAN5, EXAMPLE_ADC1_CHAN6, EXAMPLE_ADC1_CHAN7};

static int adc_raw[1][DRIVER_ADC_CH_MAX];
static int voltage[1][DRIVER_ADC_CH_MAX];
static adc_oneshot_unit_handle_t adc1_handle=NULL;
static bool do_calibration1_chan[DRIVER_ADC_CH_MAX]={false};
static adc_cali_handle_t adc1_cali_chan_handle[DRIVER_ADC_CH_MAX] = {NULL};
static bool example_adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle);
static void example_adc_calibration_deinit(adc_cali_handle_t handle);


void driver_adc_init(void)
{
    //-------------ADC1 Init---------------//
    
    adc_oneshot_unit_init_cfg_t init_config1 = {
        .unit_id = ADC_UNIT_1,
    };
    ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_config1, &adc1_handle));

    //-------------ADC1 Config---------------//
    adc_oneshot_chan_cfg_t config = {
        .bitwidth = ADC_BITWIDTH_DEFAULT,
        .atten = EXAMPLE_ADC_ATTEN,
    };
    for(int i=0;i<DRIVER_ADC_CH_MAX;i++) {
        // channel config
        ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, adc_channel[i], &config));
        //ADC1 Calibration Init
        do_calibration1_chan[i] = example_adc_calibration_init(ADC_UNIT_1, adc_channel[i], EXAMPLE_ADC_ATTEN, &adc1_cali_chan_handle[i]);
    }

    // while (1) {
    //     ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, EXAMPLE_ADC1_CHAN0, &adc_raw[0][0]));
    //     ESP_LOGD(TAG, "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, EXAMPLE_ADC1_CHAN0, adc_raw[0][0]);
    //     if (do_calibration1_chan0) {
    //         ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan0_handle, adc_raw[0][0], &voltage[0][0]));
    //         ESP_LOGD(TAG, "ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, EXAMPLE_ADC1_CHAN0, voltage[0][0]);
    //     }
    //     vTaskDelay(pdMS_TO_TICKS(1000));

    // }

}

esp_err_t driver_adc_get_value(driver_adc_channel_t channel, int *val)
{
    esp_err_t err = adc_oneshot_read(adc1_handle, adc_channel[channel], &adc_raw[0][channel]);
    if(ESP_OK != err){
        ESP_LOGW(TAG, "ADC%d Channel[%d] Read Error: %d", ADC_UNIT_1 + 1, adc_channel[channel], err);
        return err;
    }
    ESP_LOGV(TAG, "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, adc_channel[channel], adc_raw[0][channel]);
    *val = adc_raw[0][channel];
    return ESP_OK;
}

esp_err_t driver_adc_get_voltage(driver_adc_channel_t channel, int *val)
{
    // esp_err_t err = adc_oneshot_read(adc1_handle, adc_channel[channel], &adc_raw[0][channel]);
    // if(ESP_OK != err){
    //     err = adc_oneshot_read(adc1_handle, adc_channel[channel], &adc_raw[0][channel]);
    //     if(ESP_OK != err){
    //         ESP_LOGW(TAG, "ADC%d Channel[%d] Read Error: %d", ADC_UNIT_1 + 1, adc_channel[channel], err);
    //         return err;
    //     }
    // }

    int adc_avg=0;
    for(int i=0;i<3;){
        if(ESP_OK == adc_oneshot_read(adc1_handle, adc_channel[channel], &adc_raw[0][channel])){
            i++;
        }
        adc_avg += adc_raw[0][channel];
    }
    adc_avg /= 3;
    ESP_LOGD(TAG, "Channel[%d] Raw Data: %d", channel, adc_avg);
    if (do_calibration1_chan[channel]) {
        ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan_handle[channel], adc_avg, &voltage[0][channel]));
        ESP_LOGD(TAG, "Channel[%d] Cali Voltage: %d mV", channel, voltage[0][channel]);
    }
    *val = voltage[0][channel];
    return ESP_OK;
}
#endif
/*---------------------------------------------------------------
        ADC Calibration
---------------------------------------------------------------*/
static bool example_adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle)
{
    adc_cali_handle_t handle = NULL;
    esp_err_t ret = ESP_FAIL;
    bool calibrated = false;

#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    if (!calibrated) {
        ESP_LOGD(TAG, "calibration scheme version is %s", "Curve Fitting");
        adc_cali_curve_fitting_config_t cali_config = {
            .unit_id = unit,
            .chan = channel,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle);
        if (ret == ESP_OK) {
            calibrated = true;
        }
    }
#endif

#if ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    if (!calibrated) {
        ESP_LOGD(TAG, "calibration scheme version is %s", "Line Fitting");
        adc_cali_line_fitting_config_t cali_config = {
            .unit_id = unit,
            .atten = atten,
            .bitwidth = ADC_BITWIDTH_DEFAULT,
        };
        ret = adc_cali_create_scheme_line_fitting(&cali_config, &handle);
        if (ret == ESP_OK) {
            calibrated = true;
        }
    }
#endif

    *out_handle = handle;
    if (ret == ESP_OK) {
        ESP_LOGD(TAG, "Calibration Success");
    } else if (ret == ESP_ERR_NOT_SUPPORTED || !calibrated) {
        ESP_LOGW(TAG, "eFuse not burnt, skip software calibration");
    } else {
        ESP_LOGE(TAG, "Invalid arg or no memory");
    }

    return calibrated;
}

static void example_adc_calibration_deinit(adc_cali_handle_t handle)
{
#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED
    ESP_LOGD(TAG, "deregister %s calibration scheme", "Curve Fitting");
    ESP_ERROR_CHECK(adc_cali_delete_scheme_curve_fitting(handle));

#elif ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED
    ESP_LOGD(TAG, "deregister %s calibration scheme", "Line Fitting");
    ESP_ERROR_CHECK(adc_cali_delete_scheme_line_fitting(handle));
#endif
}


#define EXAMPLE_ADC_UNIT                    ADC_UNIT_1
#define _EXAMPLE_ADC_UNIT_STR(unit)         #unit
#define EXAMPLE_ADC_UNIT_STR(unit)          _EXAMPLE_ADC_UNIT_STR(unit)
#define EXAMPLE_ADC_CONV_MODE               ADC_CONV_SINGLE_UNIT_1
#define EXAMPLE_ADC_ATTEN                   ADC_ATTEN_DB_2_5
#define EXAMPLE_ADC_BIT_WIDTH               SOC_ADC_DIGI_MAX_BITWIDTH

#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2
#define EXAMPLE_ADC_OUTPUT_TYPE             ADC_DIGI_OUTPUT_FORMAT_TYPE1
#define EXAMPLE_ADC_GET_CHANNEL(p_data)     ((p_data)->type1.channel)
#define EXAMPLE_ADC_GET_DATA(p_data)        ((p_data)->type1.data)
#else
#define EXAMPLE_ADC_OUTPUT_TYPE             ADC_DIGI_OUTPUT_FORMAT_TYPE2
#define EXAMPLE_ADC_GET_CHANNEL(p_data)     ((p_data)->type2.channel)
#define EXAMPLE_ADC_GET_DATA(p_data)        ((p_data)->type2.data)
#endif

#define EXAMPLE_READ_LEN                    1024

static bool do_calibration1_chan[DRIVER_ADC_CH_MAX]={false};
static adc_cali_handle_t adc1_cali_chan_handle[DRIVER_ADC_CH_MAX] = {NULL};
static adc_channel_t channel[] = {EXAMPLE_ADC1_CHAN3, EXAMPLE_ADC1_CHAN4, EXAMPLE_ADC1_CHAN5, EXAMPLE_ADC1_CHAN6, EXAMPLE_ADC1_CHAN7};
static TaskHandle_t s_task_handle;

// 定义常量
#define TWO_PI 6.283185307179586476925286766559
#define SAMPLE_RATE 10000
// 滤波器状态变量
static float y_prev[DRIVER_ADC_CH_MAX] = {0.0f}; // 上一个输出
static float alpha[DRIVER_ADC_CH_MAX] = {0.0f};  // 滤波系数
static float filtered_value[DRIVER_ADC_CH_MAX]={0.0f}; // 输出滤波结果

// 初始化滤波器函数
static void init_low_pass_filter(uint8_t channel_num, float cutoff_frequency, float sample_rate) {
    // 计算alpha：时间常数的离散化形式
    float RC = 1.0f / (TWO_PI * cutoff_frequency); // RC时间常数
    float dt = 1.0f / sample_rate;                 // 采样时间间隔
    alpha[channel_num] = dt / (RC + dt);
}

// 滤波器函数
static float low_pass_filter(uint8_t channel_num, float input) {
    // 应用一阶IIR低通滤波器公式
    float output = alpha[channel_num] * input + (1.0f - alpha[channel_num]) * y_prev[channel_num];
    
    // 更新上一次的输出
    y_prev[channel_num] = output;

    return output;
}

static bool IRAM_ATTR s_conv_done_cb(adc_continuous_handle_t handle, const adc_continuous_evt_data_t *edata, void *user_data)
{
    BaseType_t mustYield = pdFALSE;
    //Notify that ADC continuous driver has done enough number of conversions
    vTaskNotifyGiveFromISR(s_task_handle, &mustYield);

    return (mustYield == pdTRUE);
}

static void continuous_adc_init(adc_channel_t *channel, uint8_t channel_num, adc_continuous_handle_t *out_handle)
{
    adc_continuous_handle_t handle = NULL;

    adc_continuous_handle_cfg_t adc_config = {
        .max_store_buf_size = 2048,
        .conv_frame_size = EXAMPLE_READ_LEN,
    };
    ESP_ERROR_CHECK(adc_continuous_new_handle(&adc_config, &handle));

    adc_continuous_config_t dig_cfg = {
        .sample_freq_hz = 10*1000,
        .conv_mode = EXAMPLE_ADC_CONV_MODE,
        .format = EXAMPLE_ADC_OUTPUT_TYPE,
    };

    adc_digi_pattern_config_t adc_pattern[SOC_ADC_PATT_LEN_MAX] = {0};
    dig_cfg.pattern_num = channel_num;
    for (int i = 0; i < channel_num; i++) {
        adc_pattern[i].atten = EXAMPLE_ADC_ATTEN;
        adc_pattern[i].channel = channel[i] & 0x7;
        adc_pattern[i].unit = EXAMPLE_ADC_UNIT;
        adc_pattern[i].bit_width = EXAMPLE_ADC_BIT_WIDTH;

        ESP_LOGD(TAG, "adc_pattern[%d].atten is :%"PRIx8, i, adc_pattern[i].atten);
        ESP_LOGD(TAG, "adc_pattern[%d].channel is :%"PRIx8, i, adc_pattern[i].channel);
        ESP_LOGD(TAG, "adc_pattern[%d].unit is :%"PRIx8, i, adc_pattern[i].unit);
    }
    dig_cfg.adc_pattern = adc_pattern;
    ESP_ERROR_CHECK(adc_continuous_config(handle, &dig_cfg));

    *out_handle = handle;
}

void driver_adc_task(void *arg)
{
    esp_err_t ret;
    uint32_t ret_num = 0;
    uint8_t *result = (uint8_t*) malloc(EXAMPLE_READ_LEN);
    memset(result, 0xcc, EXAMPLE_READ_LEN);

    s_task_handle = xTaskGetCurrentTaskHandle();

    adc_continuous_handle_t handle = NULL;
    continuous_adc_init(channel, sizeof(channel) / sizeof(adc_channel_t), &handle);

    adc_continuous_evt_cbs_t cbs = {
        .on_conv_done = s_conv_done_cb,
    };
    ESP_ERROR_CHECK(adc_continuous_register_event_callbacks(handle, &cbs, NULL));
    ESP_ERROR_CHECK(adc_continuous_start(handle));

    while (1) {

        /**
         * This is to show you the way to use the ADC continuous mode driver event callback.
         * This `ulTaskNotifyTake` will block when the data processing in the task is fast.
         * However in this example, the data processing (print) is slow, so you barely block here.
         *
         * Without using this event callback (to notify this task), you can still just call
         * `adc_continuous_read()` here in a loop, with/without a certain block timeout.
         */
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        char unit[] = EXAMPLE_ADC_UNIT_STR(EXAMPLE_ADC_UNIT);

        while (1) {
            ret = adc_continuous_read(handle, result, EXAMPLE_READ_LEN, &ret_num, 0);
            if (ret == ESP_OK) {
                ESP_LOGD("TASK", "ret is %x, ret_num is %"PRIu32" bytes", ret, ret_num);
                uint32_t data_avg[DRIVER_ADC_CH_MAX];
                memset(data_avg, 0, sizeof(data_avg));
                uint16_t count[DRIVER_ADC_CH_MAX];
                memset(count, 0, sizeof(count));
                for (int i = 0; i < ret_num; i += SOC_ADC_DIGI_RESULT_BYTES) {
                    adc_digi_output_data_t *p = (adc_digi_output_data_t*)&result[i];
                    uint32_t chan_num = EXAMPLE_ADC_GET_CHANNEL(p);
                    uint32_t data = EXAMPLE_ADC_GET_DATA(p);
                    /* Check the channel number validation, the data is invalid if the channel num exceed the maximum channel */
                    if (chan_num < SOC_ADC_CHANNEL_NUM(EXAMPLE_ADC_UNIT)) {
                        ESP_LOGD(TAG, "Unit: %s, Channel: %"PRIu32", Value: %"PRIu32, unit, chan_num, data);
                        switch(chan_num){
                            case EXAMPLE_ADC1_CHAN3:
                                data_avg[DRIVER_ADC_CH_BAT] += data;
                                count[DRIVER_ADC_CH_BAT]++;
                                break;
                            case EXAMPLE_ADC1_CHAN4:
                                data_avg[DRIVER_ADC_CH_AI_1] += data;
                                count[DRIVER_ADC_CH_AI_1]++;
                                break;
                            case EXAMPLE_ADC1_CHAN5:
                                data_avg[DRIVER_ADC_CH_AI_2] += data;
                                count[DRIVER_ADC_CH_AI_2]++;
                                break;
                            case EXAMPLE_ADC1_CHAN6:
                                data_avg[DRIVER_ADC_CH_AI_3] += data;
                                count[DRIVER_ADC_CH_AI_3]++;
                                break;
                            case EXAMPLE_ADC1_CHAN7:
                                data_avg[DRIVER_ADC_CH_PT100] += data;
                                count[DRIVER_ADC_CH_PT100]++;
                                break;
                        }
                    } else {
                        ESP_LOGW(TAG, "Invalid data [%s_%"PRIu32"_%"PRIx32"]", unit, chan_num, data);
                    }
                }

                for(int i=0;i<DRIVER_ADC_CH_MAX;i++){
                    data_avg[i] = data_avg[i] / count[i];
                    ESP_LOGD(TAG, "Channel[%d] Raw Data: %"PRIu32", Count: %"PRIu16, i, data_avg[i], count[i]);
                    if(do_calibration1_chan[i]){
                        int data_mv = 0;
                        ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan_handle[i], data_avg[i], &data_mv));
                        filtered_value[i] = low_pass_filter(i, data_mv);
                        // 打印alpha值，以便检查
                        // ESP_LOGI(TAG, "Alpha: %f", alpha);
                        ESP_LOGD(TAG, "Channel[%d] Cali Voltage: %d mV, %f", i, data_mv, filtered_value[i]);
                    }
                }

                /**
                 * Because printing is slow, so every time you call `ulTaskNotifyTake`, it will immediately return.
                 * To avoid a task watchdog timeout, add a delay here. When you replace the way you process the data,
                 * usually you don't need this delay (as this task will block for a while).
                 */
                vTaskDelay(1);
            } else if (ret == ESP_ERR_TIMEOUT) {
                //We try to read `EXAMPLE_READ_LEN` until API returns timeout, which means there's no available data
                break;
            }
        }
    }

    ESP_ERROR_CHECK(adc_continuous_stop(handle));
    ESP_ERROR_CHECK(adc_continuous_deinit(handle));
    free(result);
    vTaskDelete(NULL);
}

void driver_adc_init(void)
{

    for(int i=0;i<DRIVER_ADC_CH_MAX;i++) {
        //ADC1 Calibration Init
        do_calibration1_chan[i] = example_adc_calibration_init(ADC_UNIT_1, channel[i], EXAMPLE_ADC_ATTEN, &adc1_cali_chan_handle[i]);
    }
    // 给定采样率为50kHz
    float sample_rate = 39.0f;
    // 我们希望保留低于10Hz的数据
    float cutoff_frequency = 1.0f;
    // 初始化滤波器
    for(int i=0;i<DRIVER_ADC_CH_MAX;i++){
        init_low_pass_filter(i, cutoff_frequency, sample_rate);
    }
    xTaskCreatePinnedToCore(driver_adc_task, "driver_adc_task", 3*1024, NULL, 1, NULL, 1);

}

esp_err_t driver_adc_get_voltage(driver_adc_channel_t channel, float *val){
    *val = filtered_value[channel];
    return ESP_OK;
}